#include "Control.h"
#include <Generic.h>
#include <highgui.h>
#include <ctype.h>
#include <luke.h>

#include <cv.h>
#include <math.h>
//w was 0.4, v was 0.05
CControl::CControl( )
: mScreenX( 320 )
, mScreenY( 240 )
, mDistance( 8 )
, mVMax( 0.03 )
, mWMax( 0.2 )
, mAvoidRotation( 0 )
{
  mN[ 0 ] = 0;
  mN[ 1 ] = 0;
  mN[ 2 ] = -1;
}

SResult
CControl::GenerateControlSignal
(
  const cv::Mat&       rHomography
, int&            rLeft
, int&            rRight
, std::ofstream&  rFile
, bool&           rNonFatal
, double&           ObjectdistanceX
, double&           ObjectdistanceY
)
{
  SResult Result;
  rNonFatal = false;

  Generic::debugf( rFile, "[ Generate Control Signal ]\n" );
  Generic::debugf
  ( 
    rFile
  , "H:     [ %f ],\t [ %f ], [ %f ] \n" \
    "       [ %f ],\t [ %f ], [ %f ] \n" \
    "       [ %f ],\t [ %f ], [ %f ] \n" 
  , rHomography.at<double>(0, 0) 
  , rHomography.at<double>(0, 1) 
  , rHomography.at<double>(0, 2) 
  , rHomography.at<double>(1, 0) 
  , rHomography.at<double>(1, 1) 
  , rHomography.at<double>(1, 2) 
  , rHomography.at<double>(2, 0) 
  , rHomography.at<double>(2, 1) 
  , rHomography.at<double>(2, 2) 
  );

  cv::Mat K( 2, 2, CV_64F );
  K.at<double>(0,0) = 2.5;
  K.at<double>(0,1) = 0.0;
  K.at<double>(1,0) = 0.0;
  K.at<double>(1,1) = 5.0;
  
  Generic::debugf
  ( 
    rFile
  , "K:     [ %f ] [ %f ] \n" \
    "       [ %f ] [ %f ] \n"
  , K.at<double>(0,0)
  , K.at<double>(0,1)
  , K.at<double>(1,0)
  , K.at<double>(1,1) 
  ); 
  
  // Create matrix for control calculations
  cv::Mat LInverse( 2, 2, CV_64F );
  double H13 = rHomography.at<double>( 0, 2);
  double H33 = rHomography.at<double>( 2, 2);

  // L Matrix used in all papers
  cv::Mat L( 2, 2, CV_64F );
  L.at<double>(0,0) = 0;
  L.at<double>(0,1) = mScreenX * H33;
  L.at<double>(1,0) = static_cast<double>( mN[ 2 ] ) / static_cast<double>( mDistance );
  L.at<double>(1,1) = - ( H13 / mScreenX );
  LInverse = L.inv(); 
  Generic::debugf
  ( 
    "det(L):\t%f, det(L-1):\t%f\n"
  , cv::determinant( L )
  , cv::determinant( LInverse )
  );

  Result.SetResult
  (
    0 != cv::determinant( LInverse ) 
  ) << "Skipping fucked-up determinat: " << cv::determinant( LInverse );
  if( !Result )
  {
    rNonFatal = true;
  }

  if( Result )
  {
    Generic::debugf
    ( 
      rFile
    , "L:     [ %f ] [ %f ]\n" \
      "       [ %f ] [ %f ] \n"
    , L.at<double>(0,0)
    , L.at<double>(0,1)
    , L.at<double>(1,0)
    , L.at<double>(1,1) 
    ); 
    Generic::debugf
    ( 
      rFile
    , "L^-1:  [ %f ] [ %f ]\n" \
      "       [ %f ] [ %f ] \n"
    , LInverse.at<double>(0,0)
    , LInverse.at<double>(0,1)
    , LInverse.at<double>(1,0)
    , LInverse.at<double>(1,1) 
    ); 

    cv::Mat H( 2, 1, CV_64F );
    H.at<double>(0,0) = H13;
    H.at<double>(1,0) = H33 - 1;
    Generic::debugf
    ( 
      rFile
    , "H13:   [ %f ]\n"\
      "H33-1: [ %f ]\n"
    , H.at<double>(0,0)
    , H.at<double>(1,0) 
    ); 

    // Calculate Control Signal 
    cv::Mat ControlSignal( 2, 1, CV_64F );
    cv::Mat C1( 2, 1, CV_64F );
    cv::Mat C2( 2, 1, CV_64F );
    cv::gemm( -LInverse, K, 1.0, cv::Mat( 2, 2, CV_64F), 0.0, C1 );
    
    Generic::debugf
    (
      rFile
    , "C1 = -LInverse * K\n"\
      "C1:    [ %f ]\n"\
      "       [ %f ]\n"
    , C1.at<double>(0,0)
    , C2.at<double>(1,0) 
    ); 
    
    cv::gemm( C1, H, 1.0, cv::Mat( 2, 2, CV_64F), 0.0, ControlSignal );
    Generic::debugf
    (
      rFile
    , "CS = C1 * (H13, H33-1)^-1\n"\
      "Cs:    [ %f ]\n"\
      "       [ %f ]\n"
    , ControlSignal.at<double>(0,0)
    , ControlSignal.at<double>(1,0) 
    ); 
    // Scale controller values
    double& rControlV = ControlSignal.at<double>( 0, 0 );
    double& rControlW = ControlSignal.at<double>( 1, 0 );
    
    Generic::debugf
    (
      rFile
    , "V      [ %f ]\n"\
      "W      [ %f ]\n"
    , rControlV
    , rControlW 
    ); 
    rControlV *= 0.0167;
    rControlW *= 1.0;

    Generic::debugf
    (
      rFile
    , "After compensation\n"\
      "V      [ %f ]\n"\
      "W      [ %f ]\n"
    , rControlV
    , rControlW 
    ); 
    
    // PC Tim
    // rControlV = tanh( 0.95 * rControlV ) * VMax;
    // rControlW = tanh( 0.95 * rControlW ) * WMax;
    // PC Luke
    rControlV = tanh( 0.5 * rControlV ) * mVMax;
    rControlW = tanh( 0.5 * rControlW ) * mWMax;
    Generic::debugf
    (
      rFile
    , "tanh( 0.5 [V,W] ) * [V,W]Max\n"\
      "V      [ %f ]\n"\
      "W      [ %f ]\n"
    , rControlV
    , rControlW 
    ); 
    
    // Swapped Left and Right in respect to Matlab code 
    rLeft  = static_cast<int>( ( rControlV - ( rControlW * 0.053 / 2.0 ) ) / 0.000142);  
    rRight = static_cast<int>( ( rControlV + ( rControlW * 0.053 / 2.0 ) ) / 0.000142); 
    //printf( "\nrLeft: %d, \trRight: %d, \tdiff:  %d\t", rLeft, rRight, rLeft-rRight );

    if(ObjectdistanceX!=0.0 && ObjectdistanceX<20 ){
      if(ObjectdistanceY>0.0 && ObjectdistanceY<7 ){
        printf("Nondepien object in de pad, Adaptation to the left \n");
        rLeft*=0.8;
        // zet de rotatievar op links
        mAvoidRotation=-1;
      }
      else if(ObjectdistanceY<0.0 && ObjectdistanceY>-7){
         printf("Nondepien object in de pad, Adaptation to the right \n");
         rRight*=0.8;
         //zet de rotatievar op rechts
         mAvoidRotation=1;
      }
    }



    Generic::debugf
    (
      rFile
    , "Left   [ %d ]\n"\
      "Right  [ %d ]\n"
    , rLeft
    , rRight 
    );
  }
  return Result;
}

void
CControl::SmoothControlSignal( int& rLeft, int& rRight, int ClipDiff )
{
  static int LeftPrev, RightPrev;
  Generic::debugf
  ( 
    "\n=> SmoothControlSignal: pl:%d, pr:%d, l:%d, r:%d"
  , LeftPrev, RightPrev, rLeft, rRight 
  ); 
  if( abs( ( rLeft - LeftPrev ) ) > ClipDiff )
  {
    rLeft = LeftPrev + ( ClipDiff * ( 0 <= ( rLeft - LeftPrev ) ? 1 : -1 ) );
  }
  if( abs( ( rRight - RightPrev ) ) > ClipDiff )
  {
    rRight = RightPrev + ( ClipDiff * ( 0 <= ( rRight - RightPrev ) ? 1 : -1 ) );
  }
  Generic::debugf
  ( 
    "\n<= SmoothControlSignal: l:%d, r:%d"
  , rLeft, rRight 
  ); 

  LeftPrev  = rLeft;
  RightPrev = rRight;
}

void CControl::SetAlpha( unsigned ScreenX, unsigned ScreenY )
{
  mScreenX = ScreenX;
  mScreenY = ScreenY;
}


int& CControl::GetAvoidRotation(void)
{
    return mAvoidRotation;
}